#include "robotmotiontrack.h"

RobotMotionTrack::RobotMotionTrack(Robot* robot, QObject *parent) :
    QParallelAnimationGroup(parent),
    _robot(robot),
    _curKeyFrame(0),
    _oldDuration(0)
{
    foreach (int jointID, _robot->jointIDs()) {
        ensureJointTrack(jointID);
    }
}

RobotJointMotionTrack* RobotMotionTrack::ensureJointTrack(int jointID) {
    if (!_jointTracks.contains(jointID)) {
        RobotJointMotionTrack* newJointTrack;
        newJointTrack = new RobotJointMotionTrack(_robot, jointID, this);
        newJointTrack->setObjectName("track_" + QString::number(jointID));
        connect(newJointTrack, SIGNAL(keyFrame(int)), this, SLOT(onTrackKeyFrame(int)));
        connect(newJointTrack, SIGNAL(durationChanged(int)), this, SLOT(onTrackDurationChanged(int)));
        _jointTracks.insert(jointID, newJointTrack);
        addAnimation(newJointTrack);
        return newJointTrack;
    } else {
        return _jointTracks.find(jointID).value();
    }
}

RobotJointMotionTrack* RobotMotionTrack::getJointTrack(int jointID) const {
    if (_jointTracks.contains(jointID)) {
        return _jointTracks.find(jointID).value();
    } else {
        return NULL;
    }
}

void RobotMotionTrack::setRobot(Robot *robot) {
    QList<int> newJoints = robot->jointIDs();
    _robot = robot;

    //Set the robot for all of the existing joint tracks
    // In the case that the robot doesn't have the proper jointID, then the setJoint functions
    // will simply fail silently. By retaining these tracks, assigning a NullBot doesn't delete
    // the entire contents of the animation.
    foreach (int jointID, _jointTracks.keys()) {
        getJointTrack(jointID)->setRobot(_robot);
    }

    foreach (int jointID, newJoints) {
        ensureJointTrack(jointID);
    }
}

RobotMotionTrack::~RobotMotionTrack(){
    qDebug() << "Robot motion track destructor";
}


QList<int> RobotMotionTrack::jointIDs() const{
    return _robot->jointIDs();
}

QList<int> RobotMotionTrack::activeJointIDs() const{
    //Return a list of joint IDs of the joints that are actually being manipulated
    //(i.e., they have key frames)
    RobotJointMotionTrack* curJointTrack;
    QList<int> retList;
    foreach (int jointID, _jointTracks.keys()) {
        curJointTrack = getJointTrack(jointID);
        if (curJointTrack->keyValues().count() > 2) {
            retList.append(jointID);
        }
    }

    return retList;
}


void RobotMotionTrack::updateCurrentTime(int currentTime){
    //Set the robot in batch mode, this will smooth out
    //the motion
    _robot->startBatch();
    //Call the parent function to animate everything
    QParallelAnimationGroup::updateCurrentTime(currentTime);
    _robot->endBatch();

    //Once the parent has animated all the joints in parallel
    //emit the time signal
    emit timeChanged(currentTime);
}

void RobotMotionTrack::setDuration(int msec){
    //Cycle through each joint motion track and set its duration
    foreach (RobotJointMotionTrack* jointTrack, _jointTracks.values()) {
        jointTrack->setDuration(msec);
    }

    emit motionDurationChanged(msec, _oldDuration);
    _oldDuration = duration();
}


/**********************************************/
/*            Key Frame Functions             */
/**********************************************/
MotionKeyFrame RobotMotionTrack::insertKeyFrameAt(int msec){
    //Manipulating the key frames is going to change the position
    //of the robot, so tell it to do it in a batch
    _robot->startBatch();

    //Insert into all tracks
    float newValue;
    bool overallSuccess = false;
    MotionKeyFrame retValue;
    QPair<int, float> curValue;

    foreach (int jointID, _jointTracks.keys()) {
        newValue = getJointTrack(jointID)->insertKeyFrameAt(msec);
        if (newValue != INVALID_KEYFRAME) {
            overallSuccess = true;
            emit keyFrameSet(msec, jointID, newValue);
        }

        curValue.first = jointID;
        curValue.second = newValue;
        retValue.jointValues.append(curValue);
    }
    if(overallSuccess){
        //At least one key frame was created,
        //so emit the keyFrameCreated signal
        emit keyFrameCreated(msec);
        emit static_cast<RobotActionTrackSignals*>(_wrapper)->itemCreated(msec);
    }

    _robot->endBatch();

    //Bundle up the return value
    retValue.timestamp = msec;
    return retValue;
}


float RobotMotionTrack::insertKeyFrameAt(int msec, int jointID){
    float newValue;

    newValue = ensureJointTrack(jointID)->insertKeyFrameAt(msec);
    if (newValue != INVALID_KEYFRAME) {
        emit keyFrameCreated(msec);
        emit static_cast<RobotActionTrackSignals*>(_wrapper)->itemCreated(msec);
        emit keyFrameSet(msec, jointID, newValue);
    }

    return newValue;
}


void RobotMotionTrack::setEndValue(int jointID, float theta){
    float prevEndValue, newEndValue;

    //Get the appropriate track
    RobotJointMotionTrack* jointTrack = ensureJointTrack(jointID);
    prevEndValue = jointTrack->endValue().toFloat();
    jointTrack->setEndValue(theta);
    newEndValue = jointTrack->endValue().toFloat();
    if (newEndValue != prevEndValue) {
        emit keyFrameSet(jointTrack->duration(), jointID, newEndValue);
    }
}

void RobotMotionTrack::setStartValue(int jointID, float theta){
    float prevStartValue, newStartValue;

    //Get the appropriate track
    RobotJointMotionTrack* jointTrack = ensureJointTrack(jointID);
    prevStartValue = jointTrack->startValue().toFloat();
    jointTrack->setStartValue(theta);
    newStartValue = jointTrack->startValue().toFloat();
    if (newStartValue != prevStartValue) {
        emit keyFrameSet(0, jointID, newStartValue);
    }
}

float RobotMotionTrack::setKeyFrameAt(int msec, int jointID, float theta){
    float newAngle;

    //Get the appropriate track
    RobotJointMotionTrack* jointTrack = ensureJointTrack(jointID);

    bool didExist = keyFrameExists(msec);

    newAngle = jointTrack->setKeyFrameAt(msec, theta);
    if (newAngle != INVALID_KEYFRAME) {
        if (!didExist) {
            emit keyFrameCreated(msec);
            emit static_cast<RobotActionTrackSignals*>(_wrapper)->itemCreated(msec);
        }
        emit keyFrameSet(msec, jointID, newAngle);
    }

    return newAngle;
}

MotionKeyFrame RobotMotionTrack::deleteKeyFrameAt(int msec){
    _robot->startBatch();

    //Delete from all tracks
    float deletedValue;
    MotionKeyFrame retValue;
    QVector< QPair<int,float> > jointValues;
    QPair<int,float> curJointValue;

    foreach (int jointID, _jointTracks.keys()) {
        deletedValue = getJointTrack(jointID)->deleteKeyFrameAt(msec);
        if(deletedValue != INVALID_KEYFRAME){
            emit keyFrameDeleted(msec, jointID);
            //Bundle up the info to return
            curJointValue.first = jointID;
            curJointValue.second = deletedValue;
            jointValues.append(curJointValue);
        }
    }

    //The return value of the joint motion track delete function is either the
    //value that was deleted or INVALID_KEYFRAME if the key frame didn't exist
    //for the timestamp.  Either value indicates that a keyframe does not exist
    //at that point, so we can emit the general key frame deleted signal
    emit keyFrameDeleted(msec);
    emit static_cast<RobotActionTrackSignals*>(_wrapper)->itemDeleted(msec);

    _robot->endBatch();

    //Return a MotionKeyFrame struct representing the key frame that was deleted
    retValue.timestamp = msec;
    retValue.jointValues = jointValues;
    return retValue;
}

float RobotMotionTrack::deleteKeyFrameAt(int msec, int jointID){
    float deletedValue;

    //Get the appropriate track
    RobotJointMotionTrack* jointTrack = ensureJointTrack(jointID);
    deletedValue = jointTrack->deleteKeyFrameAt(msec);
    if (deletedValue != INVALID_KEYFRAME) {
        emit keyFrameDeleted(msec, jointID);
        if (!keyFrameExists(msec)) {
            //No key frames exist at the given time anymore.
            //Emit the general key frame deleted signal
            emit keyFrameDeleted(msec);
            emit static_cast<RobotActionTrackSignals*>(_wrapper)->itemDeleted(msec);
        }
    }

    return deletedValue;
}

void RobotMotionTrack::moveKeyFrame(int msec, int newMsec){
    _robot->startBatch();

    //Manipulate all tracks
    bool success, overallSuccess = false;
    foreach (int jointID, _jointTracks.keys()) {
        success = getJointTrack(jointID)->moveKeyFrame(msec, newMsec);
        if(success){
            emit keyFrameMoved(msec, jointID, newMsec);
            overallSuccess = true;
        }
    }

    //If at least one key frame was moved, we need to emit the
    //general key frame moved signal
    if(overallSuccess){
        emit keyFrameMoved(msec, newMsec);
        emit static_cast<RobotActionTrackSignals*>(_wrapper)->itemMoved(newMsec, msec);
    }

    _robot->endBatch();
}


void RobotMotionTrack::moveKeyFrame(int msec, int jointID, int newMsec){
    bool success;

    //Get the appropriate track
    RobotJointMotionTrack* jointTrack = ensureJointTrack(jointID);

    success = jointTrack->moveKeyFrame(msec, newMsec);
    if(success){
        emit keyFrameMoved(msec, jointID, newMsec);
        emit keyFrameMoved(msec, newMsec);
        emit static_cast<RobotActionTrackSignals*>(_wrapper)->itemMoved(newMsec, msec);
    }
}

float RobotMotionTrack::keyValueAt(int msec, int jointID){
    RobotJointMotionTrack* jointTrack = ensureJointTrack(jointID);

    return jointTrack->keyValueAt(msec);
}

int RobotMotionTrack::nextKeyFrame() const{
    int nextKeyFrame, curMax = duration();

    //Find the next key frame by comparing the next key frame for each of the joints.
    //Return the minimum
    foreach (RobotJointMotionTrack* jointTrack, _jointTracks.values()) {
        nextKeyFrame = jointTrack->nextKeyFrame(this->currentTime());

        curMax = (nextKeyFrame < curMax) ? nextKeyFrame : curMax;
    }

    return curMax;
}

int RobotMotionTrack::prevKeyFrame() const{
    int prevKeyFrame, curMin = 0;

    //Return the previous keyframe
    foreach (RobotJointMotionTrack* jointTrack, _jointTracks.values()) {
        prevKeyFrame = jointTrack->prevKeyFrame(this->currentTime());

        curMin = (prevKeyFrame > curMin) ? prevKeyFrame : curMin;
    }

    return curMin;
}

bool RobotMotionTrack::keyFrameExists(int msec){
    //Private function that queries the tracks to see if a keyframe
    //exists for a given time
    foreach (RobotJointMotionTrack* jointTrack, _jointTracks.values()) {
        if (jointTrack->keyValueAt(msec) != INVALID_KEYFRAME) {
            return true;
        }
    }

    return false;
}


QList<MotionKeyFrame> RobotMotionTrack::keyFrames(){
    //Gather up all of the keyframes from each of the joints
    //into a list of MotionKeyFrames.  The motion keyframes
    //are a timestamp with a list of joint positions at that
    //timestamp.  This is a different structure than how things
    //are being stored (as a collection of joint tracks with
    //keyframes in them), but is a more useful for outside users.

    //A map will be good for quick access via timestamps
    QMap<int, MotionKeyFrame> keyFrameMap;
    QList< QPair<int, float> > curJointKeyFrames;
    QPair<int, float> curJointKeyFrame, curJointValue;
    MotionKeyFrame defaultKeyFrame, curKeyFrame;

    foreach (RobotJointMotionTrack* curTrack, _jointTracks.values()) {
        //Go through each track.  If there is no motion key frame
        //for a given timestamp, create one and populate it.
        //If there is a key frame for the timestamp, append to the
        //jointValue list.
        curJointKeyFrames = curTrack->keyFrames();
        for(int i = 0; i < curJointKeyFrames.length(); i++){
            curJointKeyFrame = curJointKeyFrames[i];

            //Create the jointID/angle pair for the motion key frame
            curJointValue.first = curTrack->jointID();
            curJointValue.second = curJointKeyFrame.second;

            //Initialize the default key frame timestamp
            defaultKeyFrame.timestamp = curJointKeyFrame.first;
            //This function will now return the existing motion key frame, or, if
            //one doesn't exist, the default (blank) key frame with the timestamp set

            curKeyFrame = keyFrameMap.value(curJointKeyFrame.first, defaultKeyFrame);
            curKeyFrame.jointValues.append(curJointValue);

            keyFrameMap.insert(curJointKeyFrame.first, curKeyFrame);
        }
    }

    return keyFrameMap.values();
}

/**********************************************/
/*                    Slots                   */
/**********************************************/
void RobotMotionTrack::onTrackKeyFrame(int msec){
    //Only emit one generic key frame signal for this
    //given timestamp
    if(msec != _curKeyFrame){
        _curKeyFrame = msec;
        emit keyFrame(msec);
    }

    //Figure out what joint hit the keyframe
    int jointID = sender()->objectName().toInt();
    emit keyFrame(msec, jointID);
}

void RobotMotionTrack::onTrackDurationChanged(int msec){
    int curDuration = duration();

    if(curDuration != _oldDuration){
        emit motionDurationChanged(curDuration, _oldDuration);
        _oldDuration = curDuration;
    }
}
